#!/bin/bash

CONFIG_FILE="/etc/modalai/voxl-px4.conf"

GPS=NONE
RC=SPEKTRUM
IMU_ROTATION=NONE
OSD=DISABLE
DAEMON_MODE=DISABLE
SENSOR_CAL=ACTUAL
EXTRA_STEPS=()

# Try to source configuration variables from a file.
# This will override anything already set in the environment.
if [ -f "$CONFIG_FILE" ]; then
    echo "[INFO] Reading from $CONFIG_FILE"
    source $CONFIG_FILE
else
    echo "[INFO] $CONFIG_FILE not found, using defaults"
fi

# Make sure that the SLPI DSP test signature is there otherwise px4 cannot run
# on the DSP
if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then
    /bin/echo "Found DSP signature file"
else
    /bin/echo "[WARNING] Could not find DSP signature file"
    # Look for the DSP signature generation script
    if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
        /bin/echo "[INFO] Attempting to generate the DSP signature file"
        # Automatically generate the test signature so that px4 can run on SLPI DSP
        /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh
    else
        /bin/echo "[ERROR] Could not find the DSP signature file generation script"
        exit 0
    fi
fi

print_usage() {
    echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]"
    echo "                [-c delete configuration file and exit]"
    echo "                [-d start px4 in daemon mode]"
    echo "                [-f (Use fake rc input instead of from a real transmitter)]"
    echo "                [-i (Set IMU_ROTATION to ROTATION_YAW_180)]"
    echo "                [-m (Specify Matek GPS unit)]"
    echo "                [-o (Start OSD module on the apps processor)]"
    echo "                [-r (Specify TBS Crossfire RC receiver, MAVLINK)]"
    echo "                [-w (Specify TBS Crossfire RC receiver, raw)]"
    echo "                [-z (Use fake sensor calibration values)]"
    echo "                [-h (show help)]"

    exit 1
}

print_config_settings(){
    echo -e "\n*************************"
    echo "GPS=$GPS"
    echo "RC=$RC"
    echo "IMU_ROTATION=$IMU_ROTATION"
    echo "OSD=$OSD"
    echo "DAEMON_MODE=$DAEMON_MODE"
    echo "SENSOR_CAL=$SENSOR_CAL"
    echo "EXTRA STEPS:"
    for i in "${EXTRA_STEPS[@]}"
    do
        echo -e "\t$i"
    done
    echo -e "*************************\n"
}

while getopts "bcdhifmorwz" flag
do
    case "${flag}" in
        b) 
            echo "[INFO] Holybro GPS selected"
            GPS=HOLYBRO
            ;;
        c) 
            echo "[INFO] Wiping old config file"
            if [ -f "$CONFIG_FILE" ]; then
                rm -rf ${CONFIG_FILE}
            fi
            exit 0
            ;;
        d) 
            echo "[INFO] Enabling daemon mode"
            DAEMON_MODE=ENABLE
            ;;
        h) 
            print_usage
            ;;
        i) 
            echo "[INFO] Setting IMU_ROTATION to 4: ROTATION_YAW_180"
            IMU_ROTATION=4
            ;;
        f) 
            echo "[INFO] Setting RC to FAKE_RC_INPUT"
            RC=FAKE_RC_INPUT
            ;;
        m) 
            echo "[INFO] Matek GPS selected"
            GPS=MATEK
            ;;
        o) 
            echo "[INFO] OSD module selected"
            OSD=ENABLE
            ;;
        r) 
            echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
            RC=CRSF_MAV
            ;;
        w) 
            echo "[INFO] TBS Crossfire RC receiver, raw selected"
            RC=CRSF_RAW
            ;;
        z) 
            echo "[INFO] Fake sensor calibration values selected"
            SENSOR_CAL=FAKE
            ;;
        *) 
            print_usage
            ;;
    esac
done

if [ $DAEMON_MODE == "DISABLE" ]; then
    DAEMON=" "
else
    echo "[INFO] Daemon mode enabled"
    DAEMON="-d"
fi

if [ ! -f /data/px4/param/parameters ]; then
   echo "[INFO] Setting default parameters for PX4 on voxl"
   px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config
   /bin/sync
fi

if [ $SENSOR_CAL == "FAKE" ]; then
   /bin/echo "[INFO] Setting up fake sensor calibration values"
   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
   /bin/sync
fi

print_config_settings

GPS=$GPS RC=$RC OSD=$OSD IMU_ROTATION=$IMU_ROTATION EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start
